//****************************************************************************
//
// auto_task.c - The brains of the evalbot. This class controls how the evalbot
// will function based on inputs received. Specifically, distance IR values and
// wireless packets.

#include "inc/hw_types.h"
#include "utils/scheduler.h"
#include "drivers/motor.h"
#include "drivers/sensors.h"
#include "drivers/io.h"

#include "drive_task.h"
#include "wireless_task.h"
#include "distanceIR_task.h"
#include "helper.h"


// Define the possible states for the GOB Follower
typedef enum
{
    FOLLOWER_STATE_WAITING,		// Waiting for Instruction
    FOLLOWER_STATE_EXECUTING,	// Executing Instruction
    FOLLOWER_STATE_HALTING,		// Stopping before execution is done
} tEvalbotState;


// will add a function here. 1 will be to check the wireless buffer and set
// the state for the main loop.
void AutoTask(void *pvParam)
{
	static tEvalbotState sState = FOLLOWER_STATE_WAITING;
	static DanceInstruction instruction;

    // Process according to the current state
    switch(sState)
    {
        // WAITING - in this state the follower is waiting for a command.
        case FOLLOWER_STATE_WAITING:
        {
    		// Stop the Motors
			DriveRun(MOTOR_DRIVE_FORWARD, 0, 0);
			DriveStop();

        	if(IsInstructionWaiting())
        	{
        		// grabs the next instruction
        		instruction = GetNextInstruction();

        		// clear all the tick counts
        		DriveClearMotor(MOTOR_CLEAR_TICK);

        		// set the state so it move to execute!
        		sState = FOLLOWER_STATE_EXECUTING;
        	}

        	break;
		}

        // EXECUTING - in this state the follower is executing a command.
        case FOLLOWER_STATE_EXECUTING:
        {
        	// Check IR Distance
        	if (DistanceIRGet() < 600)
        	{
        		/* displayMessageAndNumbers("execute", (int)instruction.leftTickCount, (int)DriveTickGet(WHEEL_LEFT)); */

				// check to see if tick count was reach on the left wheel
				// if it was turn off the motor
				if (DriveTickGet(WHEEL_LEFT) >= instruction.leftTickCount) {
					instruction.leftWheelSpeed = 0;
				}

				// check to see if tick count was reach on the right wheel
				// if it was turn off the motor
				if (DriveTickGet(WHEEL_RIGHT) >= instruction.rightTickCount) {
					instruction.rightWheelSpeed = 0;
				}

				// if the motor speeds are both 0 we finished the frame
				// state is reset to waiting to get the next batch of shit to do
				if ( instruction.leftWheelSpeed == 0 && instruction.rightWheelSpeed == 0) {
					sState = FOLLOWER_STATE_WAITING;
				}

				if ( instruction.leftDirection == 1 && instruction.rightDirection == 1) {
				  //set the values for the motors to do what they need to do
				  DriveRun(MOTOR_DRIVE_FORWARD, instruction.leftWheelSpeed, instruction.rightWheelSpeed);

				} else if ( instruction.leftDirection == 0 && instruction.rightDirection == 1) {
				  DriveRun(MOTOR_DRIVE_TURN_LEFT, instruction.leftWheelSpeed, instruction.rightWheelSpeed);

				} else if ( instruction.leftDirection == 1 && instruction.rightDirection == 0) {
				  DriveRun(MOTOR_DRIVE_TURN_RIGHT, instruction.leftWheelSpeed, instruction.rightWheelSpeed);

				} else if ( instruction.leftDirection == 0 && instruction.rightDirection == 0) {
				  DriveRun(MOTOR_DRIVE_REVERSE, instruction.leftWheelSpeed, instruction.rightWheelSpeed);

				} else {
				  sState = FOLLOWER_STATE_WAITING;
				  DriveRun(MOTOR_DRIVE_FORWARD, 0, 0);
				}

        	} else // end if (DistanceIRGet() < 600)
        	{
        		// Stop the Motors
				DriveRun(MOTOR_DRIVE_FORWARD, 0, 0);
				DriveStop();

				// Set state the Halting
				sState = FOLLOWER_STATE_HALTING;
        	}
        	break;
        }

        // HALTING - in this state the follower is executing a command.
        case FOLLOWER_STATE_HALTING:
        {
        	// Check IR Distance
        	if (DistanceIRGet() < 600)
        	{
        		sState = FOLLOWER_STATE_EXECUTING;
        	}
        	break;
        }
    }
}

// This function is used to perform any needed initialization for the
// autonomous driving "task".
void AutoTaskInit(void *pvParam)
{
//    PushButtonsInit();
//    BumpSensorsInit();
}
